# -*- coding: utf-8 -*-
import argparse
import time

from sensor.Sensor import Sensor
from sound.Audio import Audio
from motion.Motion import  Motion
from vision.Vision_cv_old import  Vision
from communication.TcpClint import TcpClint
from communication.TcpServer import TcpServer
from naoqi import qi
class main_process():
    def __init__(self,ip,peer_ip):
        self.ses=qi.Session()
        self.ses.connect("tcp://"+ip+":9559")
        #初始化模块
        self.mem=self.ses.service("ALMemory")
        self.speech=self.ses.service("ALSpeechRecognition")
        self.mot=self.ses.service("ALMotion")
        self.red=self.ses.service("ALRedBallDetection")
        self.video = self.ses.service("ALVideoDevice")
        self.tts=self.ses.service("ALTextToSpeech")

        self.sensor= Sensor(self.mem)
        self.audio=Audio(self.mem,self.speech)
        self.motion=Motion(self.mot)
        self.vision=Vision(self.video)
        self.tcp_ser=TcpServer()
        self.tcp_cli=TcpClint(peer_ip)
    def prerequisite(self,char="first"):
        '''
        第一台机器人的行动指令。
        :return:
        :rtype:
        '''
        if char=="sec":
            #Server等待TCP链接。
            self.tts.say("wait for connection")
            msg=self.tcp_ser.receive()
            if msg=='1':
                self.tts.say("I got you my bro, it's my turn now")
                time.sleep(2)
            self.walk()

        elif char=="first":
            while True:
                if self.sensor.sonar_no_obstacle(): #and self.audio.wait_command():
                    print "I am ready to go"
                    break
                time.sleep(0.5)
            self.walk()

    def walk(self):
        '''
        行走过程
        1. 等待见到红球停止
        2. 行走过程中的路径修正。
        :return:
        :rtype:
        '''
        #初始开始行走。
        self.motion.move_init(0.3,0)
        # print "I'm walking"
        self.tts.say("I'm sure I's walking in a straight line, is there any red ball?")
        # self.vision.stop_flag=False
        while True:
            #见到小球停止行走
            if self.vision.redball_det():
                self.tts.say("I see the red ball, I'm stopping")
                # time.sleep(1)
                break
            elif self.sensor.sonar_no_obstacle()==False:
                self.motion.move_stop() #暂停等待，超声结束
            # elif self.vision.line_det() !=[]:
            #     control=self.vision.line_to_control()
            #     self.motion.move_ractify(0.3,0,control)#修正方向
            else:
                # #根据陀螺仪修正方向。
                # z=self.sensor.get_gyro_info()*-1
                # print z
                # self.motion.move_continue(z)
                # 根据直线修正方向。
                control = self.vision.line_to_control()
                self.motion.move_ractify(0.3, 0, control)  # 修正方向

        self.reach_the_end()
    def reach_the_end(self):
        '''
        到达终点
        1. 停驶行走
        2. 开启TCP链接，传输另一台开始信号。
        :return:
        :rtype:
        '''
        print "reach the end"
        self.motion.move_stop()
        #也可以让机器人直接蹲下。
        #创建TCP链接，并传输启动信号。
        self.tcp_cli.send()
    # def run(self):
if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="192.168.43.115",
                        help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
    parser.add_argument("--remote_ip", type=str, default="192.168.43.7",
                        help="remote IP for TCP comm")
    parser.add_argument("--char", type=str,default="first",
                        help="robot char ")

    args = parser.parse_args()
    print args.ip,args.char
    try:
        mp=main_process(args.ip,args.remote_ip)
        # mp.run()
        mp.prerequisite(args.char)
    except KeyboardInterrupt:
        del mp.vision
        del mp.audio
    except Exception as e:
        print e
    finally:
        del mp.vision
        del mp.audio
